# -*- coding: utf-8 -*-
"""
Created on Thu May 30 14:14:17 2013
Cross wind landing

Check aerodynamic coefficients
@author: Maxim
"""

import aircraft
import aerodynamics
import matplotlib.pyplot as plt
import numpy as np

def cross_wind_landing():
    """
    Performs crosswind takeoff and landing simulation.
    """
    Vcross = 10.0 #kt
    rho = 1.2255
    Vstall = 20.0
    VTo = 1.1*Vstall
    beta = 0.0
    flap = 0.0
    aileron = 15.0
    alphaGround=0.0
    steering = 0.0
    g = 9.81
    n = 25
    Cbeta = 0.025
    Pset = 100.0
    
    mu = 0.03
    mass = 620.0
    #CG = np.array([1.925,0,-0.144])
    #CG = np.array([1.813,0,-0.125])
    Vcross = Vcross*0.5144 # kt to m/s
    aileron = np.radians(aileron)
    steering = np.radians(steering)
    ac = aircraft.load('V204')
    xCG = ac.wing.get_fs_on_mac(40.0)
    CG = np.array([xCG,0,0])
    
    aero = aerodynamics.aerodynamics(ac)
    aeroData = aero.update_notrim(0.5*Vstall,rho,flap=flap,aileron=0.0,rudder=0.0,alpha=alphaGround,beta=beta)
    tm = ac.analysis.thrust
    tm.runAnalysis(0.5*Vstall,rho,Pset)
    T = tm.thrust
    cref = ac.wing.MAC
    bref = ac.wing.span
    Sref = ac.wing.area
    Xmlg = ac.landingGear.groundContact_X[2]
    Xnlg = ac.landingGear.groundContact_X[0]
    Ymlg = ac.landingGear.groundContact_Y[2]
    Zmlg = ac.landingGear.groundContact_Z[2]
    Znlg = ac.landingGear.groundContact_Z[0]
    zmg = CG[2] - Zmlg
    zng = CG[2] - Znlg
    ymg = abs(Ymlg)

    V = np.linspace(0.8*VTo,VTo,n)
    Png = np.zeros(n)
    Pmg = np.zeros(n)
    betaCross = np.zeros(n)
    rudder = np.zeros(n)
    PmgL = np.zeros(n)
    PmgR = np.zeros(n)
    PsiA = np.zeros(n)
    Ytot = np.zeros(n)
    Yng = np.zeros(n)
    YmgL = np.zeros(n)
    YmgR = np.zeros(n)

    CYb = aeroData.derivs.CYb
    CYda = aeroData.derivs.CYda*180.0/np.pi
    CYdr = aeroData.derivs.CYdr*180.0/np.pi
    Clb = aeroData.derivs.Clb
    Clda = aeroData.derivs.Clda*180.0/np.pi
    Cldr = aeroData.derivs.Cldr*180.0/np.pi
    Cnb = aeroData.derivs.Cnb
    Cnda = aeroData.derivs.Cnda*180.0/np.pi
    Cndr = aeroData.derivs.Cndr*180.0/np.pi
    Cm = aeroData.coefficients.Cm
    CD = aeroData.coefficients.CD
    CL = aeroData.coefficients.CL

    Mweight = mass*g*(CG[0] - Xmlg)
    A = np.ones([3,3])
    A[1,2] = 0.0
    A[2,0] = 0.0
    A[0,2] = mass/mu
    A[2,2] = mass*(CG[2]-Zmlg)
    b = np.zeros(3)
    A1 = np.ones([3,3])
    b1 = np.ones(3)
    for i,v in enumerate(V):
        betaCross[i] = np.arctan(Vcross/v)
        qS = rho*v*v*Sref / 2.0
        qSb = qS*bref
        Maero = Cm*qS*cref
        Mthrust = T*(-ac.landingGear.groundContact_Z[2])
        D = qS*CD
        L = qS*CL
        b[0] = (D-T*np.cos(np.radians(alphaGround)))/mu
        b[1] = mass*g-L-T*np.sin(np.radians(alphaGround))
        b[2] = Maero - Mthrust - Mweight
        x = np.linalg.solve(A,b)
        Pmg[i] = x[0]
        Png[i] = x[1]
        A1[0,0] = CYb*qS + Cbeta*(Png[i] + Pmg[i])
        A1[0,1] = 0.0
        A1[0,2] = CYdr*qS
        b1[0] = -( (CYb*betaCross[i] + CYda*aileron)*qS + Cbeta*Png[i]*steering )
        A1[1,0] = Clb*qSb - Cbeta*(Png[i]*zng + Pmg[i]*zmg)
        A1[1,1] = 2.0*ymg
        A1[1,2] = Cldr*qSb
        b1[1] = - ((Clb*betaCross[i] + Clda*aileron)*qSb - Pmg[i]*ymg - Cbeta*steering*Png[i]*zng)
        A1[2,0] = Cnb*qSb + Cbeta*(Png[i]*(Xnlg-CG[0]) - Pmg[i]*(Xmlg-CG[0]))
        A1[2,1] = 0.0
        A1[2,2] = Cndr*qSb
        b1[2] =  - ( (Cnb*betaCross[i]+Cnda*aileron)*qSb - Cbeta*steering*Png[i]*(Xnlg-CG[0]) )
        x1 = np.linalg.solve(A1,b1)
        betaCross[i] = np.degrees(betaCross[i])
        PsiA[i] = np.degrees(x1[0])
        PmgL[i] = x1[1]
        PmgR[i] = Pmg[i] - PmgL[i]
        Yng[i] = Cbeta*Png[i]
        YmgL[i] = Cbeta*PmgL[i]
        YmgR[i] = Cbeta*PmgR[i]
        Ytot[i] = Yng[i]+YmgL[i]+YmgR[i]
        rudder[i] = np.degrees(x1[2])
        #print '%.4f\t%.4f\t%.4f\t%.4f'%(np.degrees(betaCross[i]),np.degrees(x1[0]), x1[1], np.degrees(x1[2]))

    print '%.1f\t%.1f\t%.1f\t%.1f\t%.1f\t%.1f\t%.1f'%(Yng[-1],PmgL[-1],PmgR[-1],Yng[-1],YmgL[-1],YmgR[-1],Ytot[-1])

    fig1 = plt.figure(1)
    ax1 = fig1.add_subplot(211)
    ax1.hold(True)
    ax1.grid(True)
    ax1.plot(V,betaCross,'bo-')
    ax1.plot(V,PsiA,'ro-')
    ax1.legend(['beta cross','crab angle'],loc='upper left')
    ax1.set_ylim([-30,30])
    ax1.set_ylabel('deg')
    ax2 = fig1.add_subplot(212)
    ax2.hold(True)
    ax2.grid(True)
    ax2.plot(V,rudder,'bo-')
    ax2.set_ylabel('rudder defl, deg')
    ax2.set_ylim([-45,45])
    ax2.set_xlabel('Velocity,m/s')

    fig2 = plt.figure(2)
    ax3 = fig2.add_subplot(211)
    ax3.hold(True)
    ax3.grid(True)
    ax3.plot(V,Pmg/1e3,'bo-')
    ax3.plot(V,PmgL/1e3,'ro-')
    ax3.plot(V,PmgR/1e3,'go-')
    ax3.plot(V,Png/1e3,'ko-')
    ax3.legend(['main','main left','main right','nose'],loc='upper left')
    ax3.set_ylabel('Landing gear reaction, kN')
    ax3.set_ylim([-4,8])

    ax4 = fig2.add_subplot(212)
    ax4.hold(True)
    ax4.grid(True)
    ax4.set_ylabel('Side force, kN')
    ax4.plot(V,Ytot/1e3,'bo-')
    ax4.plot(V,YmgL/1e3,'ro-')
    ax4.plot(V,YmgR/1e3,'go-')
    ax4.plot(V,Yng/1e3,'ko-')
    ax4.legend(['total','main left','main right','nose'],loc='upper left')
    ax4.set_ylim([-0.05,Cbeta*mass*0.01])
    plt.show()
    
    
def cross_wind_landing_manual_input():
    """
    Performs crosswind takeoff and landing simulation.
    """
    Vcross = 10.0 #kt
    rho = 1.2255
    Vstall = 20.6
    VTo = 1.1*Vstall
    aileron = 2.0
    alphaGround=0.0
    steering = 0.0
    g = 9.81
    n = 25
    Cbeta = 0.025
    
    mu = 0.03
    mass = 620.0
    #CG = np.array([1.925,0,-0.144])
    #CG = np.array([1.813,0,-0.125])
    Vcross = Vcross*0.5144 # kt to m/s
    aileron = np.radians(aileron)
    steering = np.radians(steering)
    ac = aircraft.load('V204')
    xCG = 1.4+1.116*0.2554 #ac.wing.get_fs_on_mac(40.0)
    CG = np.array([xCG,0,0])
    
    #aero = aerodynamics.aerodynamics(ac)
    #aeroData = aero.update_notrim(0.5*Vstall,rho,flap=flap,aileron=0.0,rudder=0.0,alpha=alphaGround,beta=beta)
    T = 0
    cref = 1.166
    bref = 10.311
    Sref = 11.607
    Xmlg = ac.landingGear.groundContact_X[2]
    Xnlg = ac.landingGear.groundContact_X[0]
    Ymlg = ac.landingGear.groundContact_Y[2]
    Zmlg = ac.landingGear.groundContact_Z[2]
    Znlg = ac.landingGear.groundContact_Z[0]
    zmg = CG[2] - Zmlg
    zng = CG[2] - Znlg
    ymg = abs(Ymlg)

    V = np.linspace(0.8*VTo,VTo,n)
    Png = np.zeros(n)
    Pmg = np.zeros(n)
    betaCross = np.zeros(n)
    rudder = np.zeros(n)
    PmgL = np.zeros(n)
    PmgR = np.zeros(n)
    PsiA = np.zeros(n)
    Ytot = np.zeros(n)
    Yng = np.zeros(n)
    YmgL = np.zeros(n)
    YmgR = np.zeros(n)

    CYb = -0.541
    CYda = 0.063
    CYdr = 0.097
    Clb = -0.175
    Clda = 0.1356
    Cldr = 0.0096
    Cnb = 0.075
    Cnda = -0.0156
    Cndr = -0.0471
    Cm = -0.1925
    CD = 0.22
    CL = 1.65

    Mweight = mass*g*(CG[0] - Xmlg)
    A = np.ones([3,3])
    A[1,2] = 0.0
    A[2,0] = 0.0
    A[0,2] = mass/mu
    A[2,2] = mass*(CG[2]-Zmlg)
    b = np.zeros(3)
    A1 = np.ones([3,3])
    b1 = np.ones(3)
    for i,v in enumerate(V):
        betaCross[i] = np.arctan(Vcross/v)
        qS = rho*v*v*Sref / 2.0
        qSb = qS*bref
        Maero = Cm*qS*cref
        Mthrust = T*(-ac.landingGear.groundContact_Z[2])
        D = qS*CD
        L = qS*CL
        b[0] = (D-T*np.cos(np.radians(alphaGround)))/mu
        b[1] = mass*g-L-T*np.sin(np.radians(alphaGround))
        b[2] = Maero - Mthrust - Mweight
        x = np.linalg.solve(A,b)
        Pmg[i] = x[0]
        Png[i] = x[1]
        A1[0,0] = CYb*qS + Cbeta*(Png[i] + Pmg[i])
        A1[0,1] = 0.0
        A1[0,2] = CYdr*qS
        b1[0] = -( (CYb*betaCross[i] + CYda*aileron)*qS + Cbeta*Png[i]*steering )
        A1[1,0] = Clb*qSb - Cbeta*(Png[i]*zng + Pmg[i]*zmg)
        A1[1,1] = 2.0*ymg
        A1[1,2] = Cldr*qSb
        b1[1] = - ((Clb*betaCross[i] + Clda*aileron)*qSb - Pmg[i]*ymg - Cbeta*steering*Png[i]*zng)
        A1[2,0] = Cnb*qSb + Cbeta*(Png[i]*(Xnlg-CG[0]) - Pmg[i]*(Xmlg-CG[0]))
        A1[2,1] = 0.0
        A1[2,2] = Cndr*qSb
        b1[2] =  - ( (Cnb*betaCross[i]+Cnda*aileron)*qSb - Cbeta*steering*Png[i]*(Xnlg-CG[0]) )
        x1 = np.linalg.solve(A1,b1)
        betaCross[i] = np.degrees(betaCross[i])
        PsiA[i] = np.degrees(x1[0])
        PmgL[i] = x1[1]
        PmgR[i] = Pmg[i] - PmgL[i]
        Yng[i] = Cbeta*Png[i]
        YmgL[i] = Cbeta*PmgL[i]
        YmgR[i] = Cbeta*PmgR[i]
        Ytot[i] = Yng[i]+YmgL[i]+YmgR[i]
        rudder[i] = np.degrees(x1[2])
        #print '%.4f\t%.4f\t%.4f\t%.4f'%(np.degrees(betaCross[i]),np.degrees(x1[0]), x1[1], np.degrees(x1[2]))

    print '%.1f\t%.1f\t%.1f\t%.1f\t%.1f\t%.1f\t%.1f'%(Yng[-1],PmgL[-1],PmgR[-1],Yng[-1],YmgL[-1],YmgR[-1],Ytot[-1])

    fig1 = plt.figure(1)
    ax1 = fig1.add_subplot(211)
    ax1.hold(True)
    ax1.grid(True)
    ax1.plot(V,betaCross,'bo-')
    ax1.plot(V,PsiA,'ro-')
    ax1.legend(['beta cross','crab angle'],loc='upper left')
    ax1.set_ylim([-30,30])
    ax1.set_ylabel('deg')
    ax1.set_xlim([0.85*V[0],V[-1]])
    ax2 = fig1.add_subplot(212)
    ax2.hold(True)
    ax2.grid(True)
    ax2.plot(V,rudder,'bo-')
    ax2.set_ylabel('rudder defl, deg')
    ax2.set_ylim([-45,45])
    ax2.set_xlabel('Velocity,m/s')
    ax2.set_xlim([0.85*V[0],V[-1]])

    fig2 = plt.figure(2)
    ax3 = fig2.add_subplot(211)
    ax3.hold(True)
    ax3.grid(True)
    ax3.plot(V,Pmg/1e3,'bo-')
    ax3.plot(V,PmgL/1e3,'ro-')
    ax3.plot(V,PmgR/1e3,'go-')
    ax3.plot(V,Png/1e3,'ko-')
    ax3.legend(['main total','main left','main right','nose'],loc='upper left')
    ax3.set_ylabel('Landing gear reaction, kN')
    #ax3.set_ylim([-4,8])
    ax3.set_xlim([0.85*V[0],V[-1]])

    ax4 = fig2.add_subplot(212)
    ax4.hold(True)
    ax4.grid(True)
    ax4.set_ylabel('Side force, kN')
    ax4.plot(V,Ytot/1e3,'bo-')
    ax4.plot(V,YmgL/1e3,'ro-')
    ax4.plot(V,YmgR/1e3,'go-')
    ax4.plot(V,Yng/1e3,'ko-')
    ax4.legend(['total','main left','main right','nose'],loc='upper left')
    #ax4.set_ylim([-0.05,Cbeta*mass*0.01])
    ax4.set_xlim([0.85*V[0],V[-1]])
    ax4.set_xlabel('Velocity,m/s')
    plt.show()

if __name__=="__main__":
    cross_wind_landing_manual_input()